#include "ReconstructByKinectCommon.h"

#define FACE_THRESH 50.0

typedef unsigned short pixelvalue ;
extern pixelvalue opt_med9 (pixelvalue * p);
extern pixelvalue opt_med25 (pixelvalue * p);

IplImage *g_tempImage = NULL;
const int GAUSS_CORE_SIZE = 3;
double g_core[GAUSS_CORE_SIZE];
bool g_isCoreGenerated = false;

void doMedianFilter (IplImage *image);
void doGaussFilter (IplImage *image);


void doFilter (IplImage* depthImage)
{
	if (!g_tempImage)
		g_tempImage = cvCreateImage (cvGetSize (depthImage), depthImage->depth, 1);
	// doMedianFilter (depthImage);
	// doGaussFilter (depthImage);
}

void doMedianFilter (IplImage *image)
{
	cvCopy (image, g_tempImage);
	// 3 x 3
	for (int v = 1; v < image->height - 1; ++v) {
		for (int u = 1; u < image->width - 1; ++u) {
			unsigned short pixels[9] = {
				CV_IMAGE_ELEM (g_tempImage, unsigned short, v - 1, u - 1),
				CV_IMAGE_ELEM (g_tempImage, unsigned short, v - 1, u),
				CV_IMAGE_ELEM (g_tempImage, unsigned short, v - 1, u + 1),
				CV_IMAGE_ELEM (g_tempImage, unsigned short, v, u - 1),
				CV_IMAGE_ELEM (g_tempImage, unsigned short, v, u),
				CV_IMAGE_ELEM (g_tempImage, unsigned short, v, u + 1),
				CV_IMAGE_ELEM (g_tempImage, unsigned short, v + 1, u - 1),
				CV_IMAGE_ELEM (g_tempImage, unsigned short, v + 1, u),
				CV_IMAGE_ELEM (g_tempImage, unsigned short, v + 1, u + 1),
			};
			CV_IMAGE_ELEM (image, unsigned short, v, u) = opt_med9 (pixels);
		}
	}
// 	// 5 x 5
// 	for (int v = 2; v < image->height - 2; ++v) {
// 		for (int u = 2; u < image->width - 2; ++u) {
// 			unsigned short pixels[25] = {
// 				CV_IMAGE_ELEM (g_tempImage, unsigned short, v - 2, u - 2),
// 				CV_IMAGE_ELEM (g_tempImage, unsigned short, v - 2, u - 1),
// 				CV_IMAGE_ELEM (g_tempImage, unsigned short, v - 2, u),
// 				CV_IMAGE_ELEM (g_tempImage, unsigned short, v - 2, u + 1),
// 				CV_IMAGE_ELEM (g_tempImage, unsigned short, v - 2, u + 2),
// 				CV_IMAGE_ELEM (g_tempImage, unsigned short, v - 1, u - 2),
// 				CV_IMAGE_ELEM (g_tempImage, unsigned short, v - 1, u - 1),
// 				CV_IMAGE_ELEM (g_tempImage, unsigned short, v - 1, u),
// 				CV_IMAGE_ELEM (g_tempImage, unsigned short, v - 1, u + 1),
// 				CV_IMAGE_ELEM (g_tempImage, unsigned short, v - 1, u + 2),
// 				CV_IMAGE_ELEM (g_tempImage, unsigned short, v, u - 2),
// 				CV_IMAGE_ELEM (g_tempImage, unsigned short, v, u - 1),
// 				CV_IMAGE_ELEM (g_tempImage, unsigned short, v, u),
// 				CV_IMAGE_ELEM (g_tempImage, unsigned short, v, u + 1),
// 				CV_IMAGE_ELEM (g_tempImage, unsigned short, v, u + 2),
// 				CV_IMAGE_ELEM (g_tempImage, unsigned short, v + 1, u - 2),
// 				CV_IMAGE_ELEM (g_tempImage, unsigned short, v + 1, u - 1),
// 				CV_IMAGE_ELEM (g_tempImage, unsigned short, v + 1, u),
// 				CV_IMAGE_ELEM (g_tempImage, unsigned short, v + 1, u + 1),
// 				CV_IMAGE_ELEM (g_tempImage, unsigned short, v + 1, u + 2),
// 				CV_IMAGE_ELEM (g_tempImage, unsigned short, v + 2, u - 2),
// 				CV_IMAGE_ELEM (g_tempImage, unsigned short, v + 2, u - 1),
// 				CV_IMAGE_ELEM (g_tempImage, unsigned short, v + 2, u),
// 				CV_IMAGE_ELEM (g_tempImage, unsigned short, v + 2, u + 1),
// 				CV_IMAGE_ELEM (g_tempImage, unsigned short, v + 2, u + 2),
// 			};
// 			CV_IMAGE_ELEM (image, unsigned short, v, u) = opt_med25 (pixels);
// 		}
// 	}
}

void doGaussFilter (IplImage *image)
{
	if (!g_isCoreGenerated){
		// Generate Gauss Core
		double r = (double)GAUSS_CORE_SIZE / 2;
		for (int i = 0; i < GAUSS_CORE_SIZE; ++i) {
			float x = i - GAUSS_CORE_SIZE / 2;
			g_core[i] = exp (-0.5 * (x * x) / (r * r));
		}
		g_isCoreGenerated = true;
	}

	for (int v = 0; v < image->height; ++v) {
		for (int u = 0; u < image->width; ++u) {
			double weightSum = 0.0;
			double valueSum = 0.0;
			double hasPointsMissing = false;
			for (int i = 1; i <= GAUSS_CORE_SIZE / 2; ++i) {
				if (u + i >= image->width)
					break;
				double thisVal = CV_IMAGE_ELEM (image, unsigned short, v, u + i);
				double leftVal = CV_IMAGE_ELEM (image, unsigned short, v, u + i - 1);
				if (abs (thisVal - leftVal) > FACE_THRESH) {
					hasPointsMissing = true;
					break;
				}
				weightSum += g_core[i + GAUSS_CORE_SIZE / 2];
				valueSum += g_core[i + GAUSS_CORE_SIZE / 2] * thisVal;
			}
			if (hasPointsMissing) {
				CV_IMAGE_ELEM (g_tempImage, unsigned short, v, u) =
						CV_IMAGE_ELEM (image, unsigned short, v, u);;
				continue;
			}
			for (int i = -1; i >= -GAUSS_CORE_SIZE / 2; --i) {
				if (u + i < 0)
					break;

				double thisVal = CV_IMAGE_ELEM (image, unsigned short, v, u + i);
				double rightVal = CV_IMAGE_ELEM (image, unsigned short, v, u + i + 1);
				if (abs (thisVal - rightVal) > FACE_THRESH) {
					hasPointsMissing = true;
					break;
				}
				weightSum += g_core[i + GAUSS_CORE_SIZE / 2];
				valueSum += g_core[i + GAUSS_CORE_SIZE / 2] * thisVal;
			}
			if (hasPointsMissing) {
				CV_IMAGE_ELEM (g_tempImage, unsigned short, v, u) =
						CV_IMAGE_ELEM (image, unsigned short, v, u);;
				continue;
			}
			weightSum += g_core[GAUSS_CORE_SIZE / 2];
			valueSum += g_core[GAUSS_CORE_SIZE / 2] * CV_IMAGE_ELEM (image, unsigned short, v, u);
			CV_IMAGE_ELEM (g_tempImage, unsigned short, v, u) = valueSum / weightSum;
		}
	}
	for (int v = 0; v < image->height; ++v) {
		for (int u = 0; u < image->width; ++u) {
			double weightSum = 0.0;
			double valueSum = 0.0;
			bool hasPointsMissing = false;
			for (int i = 1; i <= GAUSS_CORE_SIZE / 2; ++i) {
				if (v + i >= image->height)
					break;
				double thisVal = CV_IMAGE_ELEM (g_tempImage, unsigned short, v + i, u);
				double upVal = CV_IMAGE_ELEM (g_tempImage, unsigned short, v + i - 1, u);
				if (abs (thisVal - upVal) > FACE_THRESH) {
					hasPointsMissing = true;
					break;
				}
				weightSum += g_core[i + GAUSS_CORE_SIZE / 2];
				valueSum += g_core[i + GAUSS_CORE_SIZE / 2] * thisVal;
			}
			if (hasPointsMissing) {
				CV_IMAGE_ELEM (image, unsigned short, v, u) =
						CV_IMAGE_ELEM (g_tempImage, unsigned short, v, u);;
				continue;
			}
			for (int i = -1; i >= -GAUSS_CORE_SIZE / 2; --i) {
				if (v + i < 0)
					break;
				double thisVal = CV_IMAGE_ELEM (g_tempImage, unsigned short, v + i, u);
				double downVal = CV_IMAGE_ELEM (g_tempImage, unsigned short, v + i + 1, u);
				if (abs (thisVal - downVal) > FACE_THRESH) {
					hasPointsMissing = true;
					break;
				}
				weightSum += g_core[i + GAUSS_CORE_SIZE / 2];
				valueSum += g_core[i + GAUSS_CORE_SIZE / 2] * thisVal;
			}
			if (hasPointsMissing) {
				CV_IMAGE_ELEM (image, unsigned short, v, u) =
						CV_IMAGE_ELEM (g_tempImage, unsigned short, v, u);;
				continue;
			}
			weightSum += g_core[GAUSS_CORE_SIZE / 2];
			valueSum += g_core[GAUSS_CORE_SIZE / 2] * CV_IMAGE_ELEM (image, unsigned short, v, u);
			CV_IMAGE_ELEM (image, unsigned short, v, u) = valueSum / weightSum;
		}
	}
}
